/*
  ******************************************************************************
  * @file    read_data_polling.c
  * @author  Sensors Software Solution Team
  * @brief   This file show how to get data from sensor.
  *
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */

  /*
   * This example was developed using the following STMicroelectronics
   * evaluation boards:
   *
   * - STEVAL_MKI109V3 + STEVAL-MKI194V1
   * - NUCLEO_F411RE + X_STEVAL-MKI194V1
   * - DISCOVERY_SPC584B + STEVAL-MKI194V1
   *
   * Used interfaces:
   *
   * STEVAL_MKI109V3    - Host side:   USB (Virtual COM)
   *                    - Sensor side: SPI(Default) / I2C(supported)
   *
   * NUCLEO_STM32F411RE - Host side: UART(COM) to USB bridge
   *                    - I2C(Default) / SPI(supported)
   *
   * DISCOVERY_SPC584B  - Host side: UART(COM) to USB bridge
   *                    - Sensor side: I2C(Default) / SPI(supported)
   *
   * If you need to run this example on a different hardware platform a
   * modification of the functions: `platform_write`, `platform_read`,
   * `tx_com` and 'platform_init' is required.
   *
   */

   /* STMicroelectronics evaluation boards definition
	*
	* Please uncomment ONLY the evaluation boards in use.
	* If a different hardware is used please comment all
	* following target board and redefine yours.
	*/

	//#define STEVAL_MKI109V3  /* little endian */
	//#define NUCLEO_F411RE    /* little endian */
	//#define SPC584B_DIS      /* big endian */

	/* ATTENTION: By default the driver is little endian. If you need switch
	 *            to big endian please see "Endianness definitions" in the
	 *            header file of the driver (_reg.h).
	 */

#ifndef ZINO_DRONE
#define ZINO_DRONE
#endif // !ZINO_DRONE


#if defined(STEVAL_MKI109V3)
	 /* MKI109V3: Define communication interface */
#define SENSOR_BUS hspi2
/* MKI109V3: Vdd and Vddio power supply values */
#define PWM_3V3 915

#elif defined(NUCLEO_F411RE)
	 /* NUCLEO_F411RE: Define communication interface */
#define SENSOR_BUS hi2c1

#elif defined(SPC584B_DIS)
	 /* DISCOVERY_SPC584B: Define communication interface */
#define SENSOR_BUS I2CD1

#endif

/* Includes ------------------------------------------------------------------*/
#include <string.h>
#include <stdio.h>
#include <stdint.h>
#include "lsm6dsr_reg.h"

#if defined(NUCLEO_F411RE)
#include "stm32f4xx_hal.h"
#include "usart.h"
#include "gpio.h"
#include "i2c.h"

#elif defined(STEVAL_MKI109V3)
#include "stm32f4xx_hal.h"
#include "usbd_cdc_if.h"
#include "gpio.h"
#include "spi.h"
#include "tim.h"

#elif defined(SPC584B_DIS)
#include "components.h"

#elif defined(ZINO_DRONE)
#include <rtthread.h>
#include "zino.h"
#include <drv_spi.h>
#include "lsm6dsr_spi2.h"
#define DBG_TAG "lsm6dsr_spi2"
#define DBG_LVL DBG_LOG
#define LSM6DSR_USE_SPI
// #define LSM6DSR_USE_I2C
#include <rtdbg.h>

#if defined(LSM6DSR_USE_SPI)
static struct rt_spi_device* spi_dev_lsm;
#define SENSOR_BUS spi_dev_lsm
// #define LSM6DSR_DEVICE_NAME "spi_lsm"
#define LSM6DSR_DEVICE_NAME "spi_lsm"
#endif
#if defined(LSM6DSR_USE_I2C)
static struct rt_i2c_device* i2c_dev_lsm;
#define SENSOR_BUS i2c_dev_lsm
#define LSM6DSR_DEVICE_NAME "i2c1"
#endif

#endif

/* Private macro -------------------------------------------------------------*/
#define    BOOT_TIME            10 //ms

/* Private variables ---------------------------------------------------------*/
static int16_t data_raw_acceleration[3];
static int16_t data_raw_angular_rate[3];
static int16_t data_raw_temperature;
static float acceleration_mg[3];
static float angular_rate_mdps[3];
static float temperature_degC;
static uint8_t whoamI, rst;
// static uint8_t tx_buffer[1000];
static stmdev_ctx_t dev_ctx={0};

/* Extern variables ----------------------------------------------------------*/

/* Private functions ---------------------------------------------------------*/

/*
 *   WARNING:
 *   Functions declare in this section are defined at the end of this file
 *   and are strictly related to the hardware platform used.
 *
 */
static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp,
	uint16_t len);
static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp,
	uint16_t len);
static void tx_com(uint8_t* tx_buffer, uint16_t len);
static void platform_delay(uint32_t ms);
static void platform_init(void);

/* Main Example --------------------------------------------------------------*/
void lsm6dsr_spi2_read_data_polling(void)
{
	// stmdev_ctx_t dev_ctx;

	platform_init();

	/* Initialize mems driver interface */
	dev_ctx.write_reg = platform_write;
	dev_ctx.read_reg = platform_read;
	/* Init test platform */
	dev_ctx.handle = SENSOR_BUS;
	/* Wait sensor boot time */
	platform_delay(BOOT_TIME);
	/* Check device ID */

	lsm6dsr_device_id_get(&dev_ctx, &whoamI);

	if (whoamI != LSM6DSR_ID)
	{
		LOG_E("id error, id(0x%x)!=LSM6DSR_ID(0x%x)!", whoamI, LSM6DSR_ID);
		return;
		// while (1);
	}
	else
	{
		LOG_I("get id 0x%x", whoamI);
	}

	/* Restore default configuration */
	lsm6dsr_reset_set(&dev_ctx, PROPERTY_ENABLE);
	do {
		lsm6dsr_reset_get(&dev_ctx, &rst);
	} while (rst);

	/* Disable I3C interface */
	lsm6dsr_i3c_disable_set(&dev_ctx, LSM6DSR_I3C_DISABLE);
	/* Enable Block Data Update */
	lsm6dsr_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
	/* Set Output Data Rate */
	lsm6dsr_xl_data_rate_set(&dev_ctx, LSM6DSR_XL_ODR_1666Hz);
	lsm6dsr_gy_data_rate_set(&dev_ctx, LSM6DSR_GY_ODR_1666Hz);
	/* Set full scale */
	lsm6dsr_xl_full_scale_set(&dev_ctx, LSM6DSR_4g);
	lsm6dsr_gy_full_scale_set(&dev_ctx, LSM6DSR_2000dps);
	/* Configure filtering chain(No aux interface)
	 * Accelerometer - LPF1 + LPF2 path
	 */
	lsm6dsr_xl_hp_path_on_out_set(&dev_ctx, LSM6DSR_LP_ODR_DIV_20);
	lsm6dsr_xl_filter_lp2_set(&dev_ctx, PROPERTY_ENABLE);


	lsm6dsr_gy_filter_lp1_set(&dev_ctx, PROPERTY_ENABLE);
	lsm6dsr_gy_lp1_bandwidth_set(&dev_ctx, LSM6DSR_STRONG);//1666Hz->
	lsm6dsr_gy_hp_path_internal_set(&dev_ctx, LSM6DSR_HP_FILTER_1Hz04);

	/* Read samples in polling mode */
	while (1) {
		uint8_t reg;
		/* Read output only if new xl value is available */
		lsm6dsr_xl_flag_data_ready_get(&dev_ctx, &reg);

		if (reg) {
			/* Read acceleration field data */
			memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
			lsm6dsr_acceleration_raw_get(&dev_ctx, data_raw_acceleration);
			acceleration_mg[0] =
				lsm6dsr_from_fs4g_to_mg(data_raw_acceleration[0]);
			acceleration_mg[1] =
				lsm6dsr_from_fs4g_to_mg(data_raw_acceleration[1]);
			acceleration_mg[2] =
				lsm6dsr_from_fs4g_to_mg(data_raw_acceleration[2]);

		}

		lsm6dsr_gy_flag_data_ready_get(&dev_ctx, &reg);

		if (reg) {
			/* Read angular rate field data */
			memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));
			lsm6dsr_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate);
			angular_rate_mdps[0] =
				lsm6dsr_from_fs2000dps_to_mdps(data_raw_angular_rate[0]);
			angular_rate_mdps[1] =
				lsm6dsr_from_fs2000dps_to_mdps(data_raw_angular_rate[1]);
			angular_rate_mdps[2] =
				lsm6dsr_from_fs2000dps_to_mdps(data_raw_angular_rate[2]);

			// sprintf((char*)tx_buffer,
			// 	"Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\t",
			// 	acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
			// tx_com(tx_buffer, strlen((char const*)tx_buffer));
			// sprintf((char*)tx_buffer,
			// 	"Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
			// 	angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
			// tx_com(tx_buffer, strlen((char const*)tx_buffer));
		}

		lsm6dsr_temp_flag_data_ready_get(&dev_ctx, &reg);

		if (reg) {
			/* Read temperature data */
			memset(&data_raw_temperature, 0x00, sizeof(int16_t));
			lsm6dsr_temperature_raw_get(&dev_ctx, &data_raw_temperature);
			temperature_degC = lsm6dsr_from_lsb_to_celsius(
				data_raw_temperature);
			// sprintf((char*)tx_buffer,
			// 	"Temperature [degC]:%6.2f\r\n", temperature_degC);
			// tx_com(tx_buffer, strlen((char const*)tx_buffer));
		}

	}
	LOG_I("init done");
}

int lsm6dsr_spi2_read_data_polling_int(void)
{
	if(dev_ctx.handle != NULL) return 0;
	platform_init();
	/* Initialize mems driver interface */
	dev_ctx.write_reg = platform_write;
	dev_ctx.read_reg = platform_read;
	/* Init test platform */
	dev_ctx.handle = SENSOR_BUS;
	/* Wait sensor boot time */
	platform_delay(BOOT_TIME);
	/* Check device ID */

	lsm6dsr_device_id_get(&dev_ctx, &whoamI);

	if (whoamI != LSM6DSR_ID)
	{
		LOG_E("id error, id(0x%x)!=LSM6DSR_ID(0x%x)!", whoamI, LSM6DSR_ID);
		return -1;
		// while (1);
	}
	else
	{
		LOG_I("get id 0x%x", whoamI);
	}

	/* Restore default configuration */
	lsm6dsr_reset_set(&dev_ctx, PROPERTY_ENABLE);
	do {
		lsm6dsr_reset_get(&dev_ctx, &rst);
	} while (rst);

	/* Disable I3C interface */
	lsm6dsr_i3c_disable_set(&dev_ctx, LSM6DSR_I3C_DISABLE);
	/* Enable Block Data Update */
	lsm6dsr_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
	/* Power mode */
	lsm6dsr_xl_power_mode_set(&dev_ctx, LSM6DSR_HIGH_PERFORMANCE_MD);
	lsm6dsr_gy_power_mode_set(&dev_ctx, LSM6DSR_GY_HIGH_PERFORMANCE);
	/* Set Output Data Rate */
	lsm6dsr_xl_data_rate_set(&dev_ctx, LSM6DSR_XL_ODR_1666Hz);
	lsm6dsr_gy_data_rate_set(&dev_ctx, LSM6DSR_GY_ODR_1666Hz);
	/* Set full scale */
	lsm6dsr_xl_full_scale_set(&dev_ctx, LSM6DSR_2g);
	lsm6dsr_gy_full_scale_set(&dev_ctx, LSM6DSR_500dps);
	/* Configure filtering chain(No aux interface)
	 * Accelerometer - LPF1 + LPF2 path
	 */
	lsm6dsr_xl_hp_path_on_out_set(&dev_ctx, LSM6DSR_LP_ODR_DIV_100);
	lsm6dsr_xl_filter_lp2_set(&dev_ctx, PROPERTY_ENABLE);

// lsm6dsr_filter_settling_mask_set
	lsm6dsr_gy_filter_lp1_set(&dev_ctx, PROPERTY_ENABLE);
	lsm6dsr_gy_lp1_bandwidth_set(&dev_ctx, LSM6DSR_LIGHT);//1666Hz->
	lsm6dsr_gy_hp_path_internal_set(&dev_ctx, LSM6DSR_HP_FILTER_16mHz);
	// lsm6dsr_gy_hp_path_internal_set
	// lsm6dsr_gy_lp1_bandwidth_set
	// lsm6dsr_xl_lp2_on_6d_set
	// lsm6dsr_xl_hp_path_on_out_set
	// lsm6dsr_xl_fast_settling_set
	//lsm6dsr_xl_hp_path_internal_set
	// lsm6dsr_gy_hp_path_internal_set

	return 0;
}
int lsm6dsr_spi2_read_gyro_adc_data(int16_t *gyroAdc)
{
	uint8_t reg;
	lsm6dsr_gy_flag_data_ready_get(&dev_ctx, &reg);
	if (reg) lsm6dsr_angular_rate_raw_get(&dev_ctx, gyroAdc);
	return reg;
}
int lsm6dsr_spi2_read_accel_adc_data(int16_t *accelAdc)
{
    uint8_t reg;
    lsm6dsr_xl_flag_data_ready_get(&dev_ctx, &reg);
    if (reg) lsm6dsr_acceleration_raw_get(&dev_ctx, accelAdc);
    return reg;
}
int lsm6dsr_spi2_read_gyro_data_polling(int32_t* mdps_x, int32_t* mdps_y, int32_t* mdps_z)
{
	uint8_t reg;
	lsm6dsr_gy_flag_data_ready_get(&dev_ctx, &reg);
	if (reg) {
		/* Read angular rate field data */
		memset(data_raw_angular_rate, 0x00, 3 * sizeof(int16_t));
		lsm6dsr_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate);
		*mdps_y = -lsm6dsr_from_fs2000dps_to_mdps(data_raw_angular_rate[0]);
		*mdps_x = -lsm6dsr_from_fs2000dps_to_mdps(data_raw_angular_rate[1]);
		*mdps_z = -lsm6dsr_from_fs2000dps_to_mdps(data_raw_angular_rate[2]);
	}
	return reg;
}

int lsm6dsr_spi2_read_accel_data_polling(int32_t *mgx, int32_t *mgy, int32_t *mgz)
{
	uint8_t reg;
	/* Read output only if new xl value is available */
	lsm6dsr_xl_flag_data_ready_get(&dev_ctx, &reg);
	if (reg) {
		/* Read acceleration field data */
		memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
		lsm6dsr_acceleration_raw_get(&dev_ctx, data_raw_acceleration);
		*mgy = -lsm6dsr_from_fs4g_to_mg(data_raw_acceleration[0])*1000.f;
		*mgx = -lsm6dsr_from_fs4g_to_mg(data_raw_acceleration[1])*1000.f;
		*mgz = -lsm6dsr_from_fs4g_to_mg(data_raw_acceleration[2])*1000.f;
	}
	return reg;
}
int lsm6dsr_spi2_read_temperature(float* degC)
{
	uint8_t reg;
	lsm6dsr_temp_flag_data_ready_get(&dev_ctx, &reg);
	if (reg) {
		/* Read temperature data */
		memset(&data_raw_temperature, 0x00, sizeof(int16_t));
		lsm6dsr_temperature_raw_get(&dev_ctx, &data_raw_temperature);
		*degC = lsm6dsr_from_lsb_to_celsius(data_raw_temperature);
	}
	return reg;
}
// ZINO_BOARD_EXPORT(lsm6dsr_read_data_polling);

/*
 * @brief  Write generic device register (platform dependent)
 *
 * @param  handle    customizable argument. In this examples is used in
 *                   order to select the correct sensor bus handler.
 * @param  reg       register to write
 * @param  bufp      pointer to data to write in register reg
 * @param  len       number of consecutive register to write
 *
 */
static int32_t platform_write(void* handle, uint8_t reg, const uint8_t* bufp,
	uint16_t len)
{
#if defined(NUCLEO_F411RE)
	HAL_I2C_Mem_Write(handle, LSM6DSR_I2C_ADD_L, reg,
		I2C_MEMADD_SIZE_8BIT, (uint8_t*)bufp, len, 1000);
#elif defined(STEVAL_MKI109V3)
	HAL_GPIO_WritePin(CS_up_GPIO_Port, CS_up_Pin, GPIO_PIN_RESET);
	HAL_SPI_Transmit(handle, &reg, 1, 1000);
	HAL_SPI_Transmit(handle, (uint8_t*)bufp, len, 1000);
	HAL_GPIO_WritePin(CS_up_GPIO_Port, CS_up_Pin, GPIO_PIN_SET);
#elif defined(SPC584B_DIS)
	i2c_lld_write(handle, LSM6DSR_I2C_ADD_L & 0xFE, reg, (uint8_t*)bufp, len);

#elif defined(ZINO_DRONE)

#if defined(LSM6DSR_USE_SPI)
	RT_ASSERT(handle != RT_NULL);
	rt_spi_send_then_send((struct rt_spi_device*)handle, &reg, 1, bufp, len);
#endif

#if defined(LSM6DSR_USE_I2C)
	RT_ASSERT(handle != RT_NULL);
	struct rt_i2c_msg msgs[2];
	msgs[0].addr = LSM6DSR_I2C_ADD_L>>1;
	msgs[0].flags = RT_I2C_WR;
	msgs[0].buf = &reg;
	msgs[0].len = 1;

	msgs[1].addr = LSM6DSR_I2C_ADD_L>>1;
	msgs[1].flags = RT_I2C_WR|RT_I2C_NO_START;
	msgs[1].buf = (rt_uint8_t *)bufp;
	msgs[1].len = len;
	rt_i2c_transfer((struct rt_i2c_bus_device*)handle, msgs, 2);
	// rt_spi_send_then_send((struct rt_spi_device*)handle, &reg, 1, bufp, len);
#endif

#endif
	return 0;
}

/*
 * @brief  Read generic device register (platform dependent)
 *
 * @param  handle    customizable argument. In this examples is used in
 *                   order to select the correct sensor bus handler.
 * @param  reg       register to read
 * @param  bufp      pointer to buffer that store the data read
 * @param  len       number of consecutive register to read
 *
 */
static int32_t platform_read(void* handle, uint8_t reg, uint8_t* bufp,
	uint16_t len)
{
#if defined(NUCLEO_F411RE)
	HAL_I2C_Mem_Read(handle, LSM6DSR_I2C_ADD_L, reg,
		I2C_MEMADD_SIZE_8BIT, bufp, len, 1000);
#elif defined(STEVAL_MKI109V3)
	reg |= 0x80;
	HAL_GPIO_WritePin(CS_up_GPIO_Port, CS_up_Pin, GPIO_PIN_RESET);
	HAL_SPI_Transmit(handle, &reg, 1, 1000);
	HAL_SPI_Receive(handle, bufp, len, 1000);
	HAL_GPIO_WritePin(CS_up_GPIO_Port, CS_up_Pin, GPIO_PIN_SET);
#elif defined(SPC584B_DIS)
	i2c_lld_read(handle, LSM6DSR_I2C_ADD_L & 0xFE, reg, bufp, len);

#elif defined(ZINO_DRONE)
#if defined(LSM6DSR_USE_I2C)
	RT_ASSERT(handle != RT_NULL);
	struct rt_i2c_msg msgs[2];
	msgs[0].addr = LSM6DSR_I2C_ADD_L>>1;
	msgs[0].flags = RT_I2C_WR;
	msgs[0].buf = &reg;
	msgs[0].len = 1;

	msgs[1].addr = LSM6DSR_I2C_ADD_L>>1;
	msgs[1].flags = RT_I2C_RD;
	msgs[1].buf = (rt_uint8_t *)bufp;
	msgs[1].len = len;
	rt_i2c_transfer((struct rt_i2c_bus_device*)handle, msgs, 2);
#endif
#if defined(LSM6DSR_USE_SPI)
	reg |= 0x80;
	RT_ASSERT(handle != RT_NULL);
	rt_spi_send_then_recv((struct rt_spi_device*)handle, &reg, 1, bufp, len);
#endif

#endif
	return 0;
}

/*
 * @brief  Send buffer to console (platform dependent)
 *
 * @param  tx_buffer     buffer to transmit
 * @param  len           number of byte to send
 *
 */
static void tx_com(uint8_t* tx_buffer, uint16_t len)
{
#if defined(NUCLEO_F411RE)
	HAL_UART_Transmit(&huart2, tx_buffer, len, 1000);
#elif defined(STEVAL_MKI109V3)
	CDC_Transmit_FS(tx_buffer, len);
#elif defined(SPC584B_DIS)
	sd_lld_write(&SD2, tx_buffer, len);
#elif defined(ZINO_DRONE)
	// rt_kput(tx_buffer, len);
#endif
}

/*
 * @brief  platform specific delay (platform dependent)
 *
 * @param  ms        delay in ms
 *
 */
static void platform_delay(uint32_t ms)
{
#if defined(NUCLEO_F411RE) | defined(STEVAL_MKI109V3)
	HAL_Delay(ms);
#elif defined(SPC584B_DIS)
	osalThreadDelayMilliseconds(ms);
#elif defined(ZINO_DRONE)
	rt_thread_mdelay(ms);
#endif
}

/*
 * @brief  platform specific initialization (platform dependent)
 */
static void platform_init(void)
{

#if defined(STEVAL_MKI109V3)
	TIM3->CCR1 = PWM_3V3;
	TIM3->CCR2 = PWM_3V3;
	HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
	HAL_Delay(1000);
#endif
#if defined(ZINO_DRONE)

#if defined(LSM6DSR_USE_I2C)
	i2c_dev_lsm = (struct rt_spi_device*)rt_device_find(LSM6DSR_DEVICE_NAME);
	if(i2c_dev_lsm)
	{
		LOG_D("%s i2c init ok!", LSM6DSR_DEVICE_NAME);
		return RT_EOK;
	}
	else
	{
		LOG_E("%s i2c init failed!", LSM6DSR_DEVICE_NAME);
		return -RT_ERROR;
	}
#endif

#if defined(LSM6DSR_USE_SPI)
	spi_dev_lsm = (struct rt_spi_device*)rt_device_find(LSM6DSR_DEVICE_NAME);
	if (spi_dev_lsm != RT_NULL)
	{
		spi_dev_lsm->config.max_hz = 10000000;
		spi_dev_lsm->config.data_width = 8;
		spi_dev_lsm->config.mode = RT_SPI_MODE_3 | RT_SPI_MSB;
		LOG_D("%s spi init ok!", LSM6DSR_DEVICE_NAME);
		return;
	}
	else
	{
		LOG_E("init failed! can't find %s spi!", LSM6DSR_DEVICE_NAME);
		return;
	}
#endif

#endif
}
